import signal

import numpy as np
import matplotlib.pyplot as plt
import struct
from scipy.signal import butter,lfilter,freqz

# y(n) = x(n) - x(n-N) + y(n-1)

# buf[ptr] = xn
# ptr++
# ptr == N

# [0,0,0,0,0]   ptr = 0
# [1,0,0,0,0]   ptr = 1
# [1,2,0,0,0]   ptr = 2
# [1,2,3,0,0]   ptr = 3
# [1,2,3,4,0]   ptr = 4
# [1,2,3,4,5]   ptr = 0
# [6,2,3,4,5]   ptr = 1
# [6,7,3,4,5]   ptr = 2

# buf[ptr] 存的是最老的数据 x(n-N)
# buf[ptr-1] 存x(n-1)
# buf[ptr-2] 存x(n-2)
# buf[ptr-N] = buf[ptr] 存x(n-N)

# y(n) = 2*y(n-1)-y(n-2)+x(n)-2*x(n-6)+x(n-12)
class ecglp:
    def __init__(self):
        self.xbuf = [0]*12
        self.yn1 = 0
        self.yn2 = 0
        self.xptr = 0

    def filter(self,xn):
        yn = self.yn1*2-self.yn2+xn-2*self.xbuf[p]+self.xbuf[self.ptr]

#数字滤波器类实现y(n) = b(0)*x(n)+b(1)*x(n-1)+b(2)*x(n-2)......-a(1)*y(n-1)-a(2)*y(n-2)
class DigitalFilter:
    #通过滤波器的类
    #通过设置系数A和系数B，来实现在线的滤波
    def __init__(self, CeofB, CeofA):
        self.OrderA = len(CeofA)
        self.OrderB = len(CeofB)
        self.bufx = [0] * self.OrderB
        self.bufy = [0] * self.OrderA
        self.ptrx = 0
        self.ptry = 0
        self.CeofB = CeofB
        self.CeofA = CeofA

    def filter(self, xn):
        s = 0
        self.bufx[self.ptrx] = xn
        p = self.ptrx
        for i in range(0,self.OrderB):
            s = s + self.bufx[p] *self.CeofB[i]
            p = p - 1
            if p < 0:
                p = self.OrderB -1
        # ptry 存着 y(n-1)
        p = self.ptry

        for i in range(1, self.OrderA):
            s = s - self.CeofA[i] * self.bufy[p]
            p = p-1
            if p<0:
                p = self.OrderA - 1

        self.ptry = self.ptry + 1
        if self.ptry == self.OrderA:
            self.ptry = 0
        self.bufy[self.ptry] = s

        self.ptrx = self.ptrx + 1
        if self.ptrx == self.OrderB:
            self.ptrx = 0
        return s


if __name__ =='__main__':
    b, a = butter(2, [10 / 125], "low")
    print(b,a)

    fp = open("D:/wuhui/testcode/SmartHealth-master/DATA/mitdb/102.dat","rb")
    fp_out = open("D:/wuhui/testcode/SmartHealth-master/DATA/mitdb/102_out.dat","wb")
    # diff = NavgFilter(5)
    # f2 = DigitalFilter( [1/5,0,0,0,0,-1/5], [1,-1])
    f2 = DigitalFilter(b,a)
    for i in range(0, 250):
        x = struct.unpack('h',fp.read(2))
        # y = diff.filter(x[0])
        y = f2.filter(x[0])
        fp_out.write(struct.pack('h',int(y)))
    fp.close()
    fp_out.close()
    ecg0 = np.fromfile("D:/wuhui/testcode/SmartHealth-master/DATA/mitdb/102.dat",dtype=np.short)
    ecg1 = np.fromfile("D:/wuhui/testcode/SmartHealth-master/DATA/mitdb/102_out.dat",dtype=np.short)
   
    # yn = yn1 + xn -x(n-5)
    ecg2 = lfilter( [1/5,0,0,0,0,-1/5], [1,-1], ecg0)
    ecg3 = lfilter(b, a, ecg0) + 10
    
    
    
    plt.subplot(2,1,1)
    plt.plot(ecg0[0:250])
    plt.subplot(2,1,2)
    plt.plot(ecg1[0:250])
    # plt.hold()
    plt.plot(ecg2[0:250])
    plt.plot(ecg3[0:250])
    plt.show()